/*
    * File      : eDriver.h
    * 
    * 提供了控制零差云控程序的接口
    * 
    * Change Logs:
    * Date                     Author         Notes
    * 2022-04 ~ 2022-06        朱治铼         控制零差云控驱动
*/

#ifndef  __EDRIVER_H__
#define  __EDRIVER_H__

#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <math.h>
#include <rtthread.h>
#include <rtdevice.h>
#include "rtconfig.h"

#ifdef RT_USING_FINSH
#include <finsh.h>
#endif

#include "canfestival.h"
#include "timers_driver.h"
#include "objDictionary.h"



// 配置从站使用的宏
#define ALL_SERVO_NODE  0
#define SERVO_1    2
#define SERVO_2    3
#define SERVO_3    4

#define PRODUCER_HEARTBEAT_TIME 5000
#define CONSUMER_HEARTBEAT_TIME 10000

#define TPDO_TRANSMISSION_SYNC(x)  (x)  // 累计经历x个同步帧，发送PDO
#define TPDO_TRANSMISSION_SYNC_0    0   // 映射数据改变且收到一个同步帧就发送PDO
#define TPDO_TRANSMISSION_SYNC_254  254 // 映射数据改变或定时器到达就发送PDO
#define RPDO_TRANSMISSION_SYNC  0       // 接收到PDO，在下一个同步帧会更新到应用
#define RPDO_TRANSMISSION_ASYN  254     // 异步：接收到就立即更新到应用

// 伺服运行模式设定
#define PROFILE_POSITION_MODE       0x01    //  轮廓位置模式
#define PROFILE_SPEED_MODE          0x03    //  轮廓速度模式
#define PROFILE_TORQUE_MODE         0x04    //  轮廓扭矩模式
#define INTERPOLATION_MODE          0x07    //  插补模式 
#define CYCLIC_SYNC_POSITION_MODE   0x08    //  周期同步位置模式
#define CYCLIC_SYNC_VELOCITY_MODE   0x09    //  周期同步速度模式
#define CYCLIC_SYNC_TORQUE_MODE     0x0A    //  周期同步力矩模式

#define ENABLE_UPDATE_TO_TAR_POS    0x1F   //  伺服使能 -> 允许更新到目标位置


// 线程延时 20ms
#define SYNC_DELAY              rt_thread_delay(1000/50)
#define EDRIVER_MDELAY(time)    rt_thread_delay(time)
#define WORD_WAIT_TIME          10

// 驱动器 TPDO 同步周期 10ms
#define SYNC_TIME   (10000)
#define SERVO_SYNC_TIME (10000)

//  启动刹车和释放刹车
#define OPEN_BRAKE      RT_TRUE
#define RELEASE_BRAKE   RT_FALSE
#define ERROR_BRAKE     2
#define NET_WRI_FAIL    -1


// 伺服信号量结构体
typedef struct servo_config_state
{
	rt_uint8_t case_cnt;    //  配置的伺服case计数
	rt_uint8_t retry_cnt;   //  重试次数 3次
    uint8_t    nodeId;      //  伺服ID
	struct rt_semaphore finish_sem; //  信号量
}servo_sem_t;

// 主站设备主结构体
typedef struct edriver_no_err_device{
    CO_Data*        od_Data;
    s_BOARD         agv_board;
    servo_sem_t       *semTable;
    void            (*MSetSYNC)(rt_uint32_t cycle);
} eDriverMaster_t;


// 初始化设备。在RT系统下自动启动 (目前是手动)
int MInitialize_eDriver(void);
static int MCanopen_init(void);

//  配置主站和从站相关接口
void MInitNodes(CO_Data* d, UNS32 id);
void MInitNodeExit(CO_Data* d, UNS32 id);
static void MConfigServo(uint8_t nodeId);
static void MConfig_Single_Servo(void *parameter);
static void MConfig_Servo_Param(uint8_t nodeId, servo_sem_t* conf);
static void MConfig_Servo_Param_Callback(CO_Data* d, UNS8 nodeId);
void canopen_start_thread_entry(void *parameter);

// 预处理配置三个从站
static int MServoConfig_Fir(servo_sem_t* conf);
static int MServoConfig_Sec(servo_sem_t* conf);
static int MServoConfig_Trd(servo_sem_t* conf);


static void MSetSYNC(rt_uint32_t cycle);    // 激活节点同步发生器并设置同步周期(ms) --内部单位为us

void MSetHeartBeat(void);   // 三个从站的心跳机制设定

//---以下为控制伺服驱动的接口

void edriver_set_control_word(rt_uint8_t nodeid, rt_uint16_t val);  //  配置从站控制字
void edriver_boot_servo(rt_uint8_t nodeid);                         //  PDO--引导从站由伺服无故障进入伺服运行
int  MGetServoStatus(void);                                         //  获取三个从站的状态（调试用）
void MSetOperationMode(rt_uint8_t   nodeid, rt_uint8_t mode);       //  设置从站的操作模式
void edriver_active_target(rt_uint8_t nodeid);                      //  激活伺服运行 -> 允许目标更新位置
int  MSetEDriverControl(rt_uint8_t nodeid);                         //  SDO--从站引导流程
int  MSDOSetPosition(rt_uint8_t nodeid, rt_uint32_t *position);             //  SDO--插补模式下设置位置
int  MInterpolationSetPosition(rt_uint8_t	nodeID, rt_uint32_t *position); //  PDO--插补模式下设置位置

/*
    函数功能：驱动器启动刹车与释放刹车接口 , 无回调函数
    参数： nodeid : 需要控制的伺服驱动器的设备ID
           open_or_shut:TRUE 为启动刹车
                        FALSE 为释放刹车
    返回值：成功：0
*/
rt_uint32_t edriver_brake_and_release_the_brake(rt_uint8_t nodeid, rt_bool_t open_or_release);

/*
    函数功能：查看错误，清除错误 , 有回调函数
    参数： nodeid : 需要控制的伺服驱动器的设备ID
    返回值：成功：0
*/
rt_uint32_t edriver_clear_error(rt_uint8_t  nodeid);

/*
    函数功能： 配置零差云控的canID , 无回调函数。适合零差云控设备刚到的时候，设备ID默认为1还会10的时候，也可以通过上位机进行修改
    参数: 
    返回值：
*/
// void edriver_config_servo_canid();

/*
    函数功能：打印从站实际位置、当前模式、状态字的信息
    参数：无
    返回值：无
*/
void edriver_print_60XX_info(void);

/*
    函数功能：设置目标位置 ， PDO模式
    参数： nodeid : 设备ID
           pos:   目标位置 
    返回值：无
*/
rt_int32_t edriver_set_target_position(rt_uint8_t  nodeid, rt_int32_t pos);

/*
    函数功能：设置速度  SDO模式
    参数： nodeid : 设备ID
           speed:   速度 
    返回值：无
*/
rt_int32_t edriver_set_speed(rt_uint8_t nodeid, rt_uint32_t  speed);

/*
    函数功能：设置目标速度  SDO模式
    参数： nodeid : 设备ID
           tar_speed:   目标速度 
    返回值：无
*/
rt_int32_t edriver_set_target_speed(rt_uint8_t nodeid, rt_int32_t  tar_speed);

/*
    函数功能：设置加速度    SDO模式
    参数： nodeid : 设备ID
           acceleration:  加速度 
    返回值：无
*/
rt_int32_t edriver_set_acceleration(rt_uint8_t nodeid, rt_uint32_t  acceleration);

/*
    函数功能： 设置减速度   SDO模式
    参数： nodeid : 设备ID
           deceleration:   减速度  
    返回值：无
*/
rt_int32_t edriver_set_deceleration(rt_uint8_t nodeid, rt_uint32_t  deceleration);

/*
    函数功能： 设置目标位置
    参数： nodeid : 设备ID
           deceleration:   减速度  
    返回值：无
    备注：  包含了csp模式，在该模式下在设置目标位置前，首先会读取对应从站的实际位置，将该位置写入目标位置，再操作第二个参数的位置。
*/
rt_int32_t edriver_sdo_set_tar_position(rt_uint8_t nodeid, rt_int32_t  pos, rt_bool_t csp_mode);

//  驱动设备复位
void edriver_position_reset(rt_uint8_t nodeid);

//  驱动进行复位
void edriver_all_servo_reset(void);

//  速度归零
void edriver_speed_reset(rt_uint8_t nodeid);


// --------------  控制命令ends

//canopen sdo 通信函数的封装
#define ReadDicCallBack(d,nodeid,mainindex,subIndexNbr,datatype,callback) \
    readNetworkDictCallback ( d, nodeid, mainindex, subIndexNbr, datatype, callback, 0)

#define GetReadResult(od_Data, nodeId, status, size, abortCode) \
    getReadResultNetworkDict(od_Data, nodeId, status, size,abortCode)

#define WriteDicCallBack(d,  nodeId,  mainindex, subIndex,  count,  dataType, data,  Callback)  \
    writeNetworkDictCallBack (d,  nodeId,  mainindex, subIndex,  count,  dataType, data,  Callback, 0)

#define GetWriteResult(od_Data, nodeId,  status,  size,  abortCode)    \
    getWriteResultNetworkDict(od_Data, nodeId,  status,  size,  abortCode)


// canopen相关注册回调函数
void master402_heartbeatError(CO_Data* d, UNS8 heartbeatID);
void master402_initialisation(CO_Data* d);
void master402_preOperational(CO_Data* d);
void master402_operational(CO_Data* d);
void master402_stopped(CO_Data* d);
void master402_post_sync(CO_Data* d);
void master402_post_TPDO(CO_Data* d);
void master402_storeODSubIndex(CO_Data* d, UNS16 wIndex, UNS8 bSubindex);
void master402_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg, const UNS8 errSpec[5]);

////    调试宏
#ifdef CANFESTIVAL_USING_KPRINTF_DEBUG

#define ERROR_DEBUG(...)    rt_kprintf("ERROR : "); rt_kprintf(__VA_ARGS__)
#define INFO_DEBUG(...)		rt_kprintf("INFO : "); rt_kprintf(__VA_ARGS__)
#define WARN_DEBUG(...)		rt_kprintf("WARNING : "); rt_kprintf(__VA_ARGS__)

#else

#define ERROR_DEBUG(...)
#define INFO_DEBUG(...)
#define WARN_DEBUG(...)

#endif



#endif
